package com.yoki.cloud.common.utils.geometry;

import java.util.ArrayList;
import java.util.LinkedList;
import java.util.List;

/**
 * @author Sky$
 * @Description: TODO
 * @date 2018/2/5$ 14:30$
 */
public class GeometryUtils {

    /**
     * 根据经纬度获取两点间的距离
     *
     * @param lng1 经度1
     * @param lat1 纬度1
     * @param lng2 经度2
     * @param lat2 纬度2
     * @return
     */
    public static double getCoordinateDistance(double lng1, double lat1, double lng2, double lat2) {
        //经度差值
        double dx = lng1 - lng2;
        //纬度差值
        double dy = lat1 - lat2;
        //平均纬度
        double b = (lat1 + lat2) / 2.0;
        //东西距离
        double Lx = Math.toRadians(dx) * 6367000.0 * Math.cos(Math.toRadians(b));
        //南北距离
        double Ly = 6367000.0 * Math.toRadians(dy);
        //用平面的矩形对角距离公式计算总距离
        return Math.sqrt(Lx * Lx + Ly * Ly);
    }

    public static double getXYDistance(Point p1, Point p2) {
        return getXYDistance(p1.getX(), p1.getY(), p2.getX(), p2.getY());
    }

    /**
     * 根据平面坐标点获取两点间的距离
     *
     * @param x1
     * @param y1
     * @param x2
     * @param y2
     * @return
     */
    public static double getXYDistance(double x1, double y1, double x2, double y2) {
        return Math.sqrt(Math.pow(Math.abs(x1 - x2), 2) + Math.pow(Math.abs(y1 - y2), 2));
    }

    /**
     * @param rssi 信号强度，为负数。
     * @param a    距离探测设备1m时的rssi值的绝对值，最佳范围在45-49之间。
     * @param n    环境衰减因子，需要测试矫正，最佳范围在3.25-4.5之间。
     * @return 距离，单位：m。
     */
    public static double getRssiDistance(final byte rssi, Double a, Double n) {
        // d=10^((ABS(RSSI)-A)/(10*n))
        // 其中d为距离，单位是m。
        // RSSI为rssi信号强度，为负数。
        // A为距离探测设备1m时的rssi值的绝对值，最佳范围在45-49之间。
        // n为环境衰减因子，需要测试矫正，最佳范围在3.25-4.5之间。
        int absRssi = Math.abs(rssi);
        double d = 0;
        if (null == a) {
            a = 45d;
        }
        if (null == n) {
            n = 4.0d;
        }
        d = absRssi - a;
        d = d / (10 * n);
        d = Math.pow(10, d);
        return d;
    }

    /**
     * 求解两圆相交的交点坐标<br>
     *
     * @param c1  圆1
     * @param c2  圆2
     * @param dev 偏差值
     * @return
     * @see #getIntersect(Point p1, double r1, Point p2, double r2, Double dev)
     */
    public static List<Point> getIntersect(Circle c1, Circle c2, Double dev) {
        return getIntersect(c1.getPoint(), c1.getR(), c2.getPoint(), c2.getR(), dev);
    }

    /**
     * 求解两圆相交的交点坐标<br>
     * 圆的参数方程
     * <ul>
     * <li> C1 : (x - x1)^2 + (y - y1)^2 = r1^2 </li>
     * <li> C2 : (x - x2)^2 + (y - y2)^2 = r2^2 </li>
     * </ul>
     * C1 - C2可得 :
     * <ul>
     * <li> C3 : x1^2 - x2^2 - 2*x(x1 - x2) + y1^2 - y2^2 - 2*y*(y1 - y2) = r1^2 - r2^2 </li>
     * 如果 y1 != y2则 :
     * <ul>
     * <li> C4 : K*x + B = y </li>
     * 其中
     * <li> K = (x1 - x2) / (y1 - y2) </li>
     * <li> B = (x1^2 - x2^2 + y1^2 -y2^2 + r2^2 - r1^2) / 2(y1 - y2) </li>
     * C4带入C1可得 :
     * <li> (1 + K*K)*x^2 - 2*[x1 + (B - y1)*K]*x +x1^2 + (B - y1)^2 - r1^2 </li>
     * </ul>
     * 如果 y1 == y2 , x1 != x2则 :
     * <ul>
     * 通过方程C3,y1 = y2,可得方程C5
     * <li> C5 : x1^2 - x2^2 - 2*x(x1 - x2) = r1^2 - r2^2 </li>
     * 可得 :
     * <li> x0 = (x1 * x1 - x2 * x2 + r2 * r2 - r1 * r1) / (2 * (x1 - x2)) </li>
     * 将x0带入方程C1 :
     * <li> C6 : y^2 - 2*y*y1 + y1^2 - r1^2 + (x0 - x1)^2 </li>
     * </ul>
     * </ul>
     *
     * @param p1
     * @param r1
     * @param p2
     * @param r2
     * @param dev
     * @return
     */
    public static List<Point> getIntersect(Point p1, double r1, Point p2, double r2, Double dev) {
        double x1 = p1.getX(), y1 = p1.getY(), x2 = p2.getX(), y2 = p2.getY();
        List<Point> list = new ArrayList<>(2);
        // 在一元二次方程中 a*x^2 + b*x + c=0
        double a, b, c;
        //判别式的值
        double delta = -1;
        // C1 : (x - x1)^2 + (y - y1)^2 = r1^2
        // C2 : (x - x2)^2 + (y - y2)^2 = r2^2
        // C1 - C2可得 :
        // C3 : x1^2 - x2^2 - 2*x(x1 - x2) + y1^2 - y2^2 - 2*y*(y1 - y2) = r1^2 - r2^2
        //如果 y1 ! = y2
        if (y1 != y2) {
            // 则 :
            // C4 : K*x + B = y
            //其中
            // K = (x1 - x2) / (y1 - y2)
            // B = (x1^2 - x2^2 + y1^2 -y2^2 + r2^2 - r1^2) / 2(y1 - y2)
            double K = (x1 - x2) / (y1 - y2);
            double B = x1 * x1 - x2 * x2 + y1 * y1 - y2 * y2 + r2 * r2 - r1 * r1;
            B /= (2 * (y1 - y2));
            // C4带入C1可得 :
            // (1 + K*K)*x^2 - 2*[x1 + (B - y1)*K]*x +x1^2 + (B - y1)^2 - r1^2
            a = 1 + K * K;
            b = -2 * (x1 + (B - y1) * K);
            c = x1 * x1 + (B - y1) * (B - y1) - r1 * r1;
            // 下面使用判定式 判断是否有解
            delta = b * b - 4 * a * c;
            if (delta > 0) {
                // 有两个解
                double x_1 = (-b + Math.sqrt(b * b - 4 * a * c)) / (2 * a);
                double x_2 = (-b - Math.sqrt(b * b - 4 * a * c)) / (2 * a);
                double y_1 = B - K * x_1;
                double y_2 = B - K * x_2;
                list.add(new Point(x_1, y_1));
                list.add(new Point(x_2, y_2));
            } else if (delta == 0) {
                // 一个解
                double x_1 = -b / (2 * a);
                double y_1 = B - K * x_1;
                list.add(new Point(x_1, y_1));
            } else {
                // 两个圆不相交
                return null;
            }
        } else if (x1 != x2) {
            // 当y1 = y2时，x的两个解相等
            // 通过方程C3,y1 = y2,可得方程C5
            // C5 : x1^2 - x2^2 - 2*x(x1 - x2) = r1^2 - r2^2
            // 求得x0 = (x1 * x1 - x2 * x2 + r2 * r2 - r1 * r1) / (2 * (x1 - x2))
            double x0 = (x1 * x1 - x2 * x2 + r2 * r2 - r1 * r1) / (2 * (x1 - x2));
            // 将x0带入方程C1 :
            // C6 : y^2 - 2*y*y1 + y1^2 - r1^2 + (x0 - x1)^2
            a = 1;
            b = -2 * y1;
            c = y1 * y1 - r1 * r1 + (x0 - x1) * (x0 - x1);
            delta = b * b - 4 * a * c;
            if (delta > 0) {
                // 两个解答
                double y_1 = (-b + Math.sqrt(b * b - 4 * a * c)) / (2 * a);
                double y_2 = (-b - Math.sqrt(b * b - 4 * a * c)) / (2 * a);
                list.add(new Point(x0, y_1));
                list.add(new Point(x0, y_2));
            } else if (delta == 0) {
                // 一个解
                double y_1 = -b / (2 * a);
                list.add(new Point(x0, y_1));
            } else {
                // 两个圆不相交
                return null;
            }
        } else {
            System.out.println("无解");
            return null;
        }
        return list;
    }

    public static List<Point> getRssiIntersect(Point p1, final byte rssi1, Point p2, final byte rssi2, Double dev, Double a, Double n) {
        double r1 = getRssiDistance(rssi1, a, n);
        double r2 = getRssiDistance(rssi2, a, n);
        return getIntersect(p1, r1, p2, r2, dev);
    }

    /**
     * 获取与目标点最短距离的点
     *
     * @param p  目标点
     * @param ps 点组
     * @return
     */
    public static Point getShortestPoint(Point p, Point... ps) {
        if (null == ps) {
            return null;
        }
        Point shortestPoint = null;
        double shortestDistance = 0;
        for (int i = 0; i < ps.length; i++) {
            Point pi = ps[i];
            if (i == 0) {
                shortestDistance = getXYDistance(p, pi);
                shortestPoint = pi;
                continue;
            }
            if (getXYDistance(p, pi) < shortestDistance) {
                shortestPoint = pi;
            }
        }
        return shortestPoint;
    }

    /**
     * 获取与目标点最短距离的点
     *
     * @param p  目标点
     * @param ps 点组
     * @return
     */
    public static Point getShortestPoint(Point p, List<Point> ps) {
        if (null == ps) {
            return null;
        }
        Point shortestPoint = null;
        double shortestDistance = 0;
        for (int i = 0; i < ps.size(); i++) {
            Point pi = ps.get(i);
            if (i == 0) {
                shortestDistance = getXYDistance(p, pi);
                shortestPoint = pi;
                continue;
            }
            if (getXYDistance(p, pi) < shortestDistance) {
                shortestPoint = pi;
            }
        }
        return shortestPoint;
    }

    /**
     * 三点定位，外心
     *
     * @param p1 点1
     * @param p2 点2
     * @param p3 点3
     * @return
     */
    public static Point getOutterPosition(Point p1, Point p2, Point p3) {
        double x = Math.pow(p1.getX(), 2) * (p2.getY() - p3.getY());
        x += Math.pow(p2.getX(), 2) * (p3.getY() - p1.getY());
        x += Math.pow(p3.getX(), 2) * (p1.getY() - p2.getY());
        x -= (p1.getY() - p2.getY()) * (p2.getY() - p3.getY()) * (p3.getY() - p1.getY());
        x /= 2;
        double temp = p1.getX() * (p2.getY() - p3.getY());
        temp += p2.getX() * (p3.getY() - p1.getY());
        temp += p3.getX() * (p1.getY() - p2.getY());
        x /= temp;

        double y = Math.pow(p1.getY(), 2) * (p2.getX() - p3.getX());
        y += Math.pow(p2.getY(), 2) * (p3.getX() - p1.getX());
        y += Math.pow(p3.getY(), 2) * (p1.getX() - p2.getX());
        y -= (p1.getX() - p2.getX()) * (p2.getX() - p3.getX()) * (p3.getX() - p1.getX());
        y /= 2;
        y /= -temp;

        Point p = new Point(x, y);
        return p;
    }

    /**
     * 三点定位，内心
     *
     * @param p1 点1
     * @param p2 点2
     * @param p3 点3
     * @return
     */
    public static Point getInnerPosition(Point p1, Point p2, Point p3) {
        double d12 = getXYDistance(p1, p2);
        double d23 = getXYDistance(p2, p3);
        double d31 = getXYDistance(p3, p1);
        double x = p1.getX() * d23 + p2.getX() * d31 + p3.getX() * d12;
        x /= (d12 + d23 + d31);
        double y = p1.getY() * d23 + p2.getY() * d31 + p3.getY() * d12;
        y /= (d12 + d23 + d31);
        Point p = new Point(x, y);
        return p;
    }

    public static Point getPosition(Point center1, final byte rssi1, Point center2, final byte rssi2, Point center3, final byte rssi3) {
        return getPosition(center1, rssi1, center2, rssi2, center3, rssi3, null, null, null);
    }

    public static Point getPosition(Point center1, final byte rssi1, Point center2, final byte rssi2, Point center3, final byte rssi3, Double dev, Double a, Double n) {
        List<List<Point>> userfulList = new LinkedList<>();
        List<Point> list1 = getRssiIntersect(center1, rssi1, center2, rssi2, dev, a, n);
        if (null == list1 || list1.size() <= 0) {
            return null;
        }
        userfulList.add(list1);
        List<Point> list2 = getRssiIntersect(center2, rssi2, center3, rssi3, dev, a, n);
        if (null == list2 || list2.size() <= 0) {
            return null;
        }
        userfulList.add(list2);
        List<Point> list3 = getRssiIntersect(center3, rssi3, center1, rssi1, dev, a, n);
        if (null == list3 || list3.size() <= 0) {

            return null;
        }
        userfulList.add(list3);
        if (null == userfulList || userfulList.size() <= 0) {
            return null;
        }
        if (userfulList.size() == 3) {
            Point p2 = getShortestPoint(userfulList.get(0).get(0), userfulList.get(1));
            Point p3 = getShortestPoint(userfulList.get(0).get(0), userfulList.get(2));
            Point p1 = getShortestPoint(p2, userfulList.get(0));
            if (null != p1 && null != p2 && null != p3) {
                Point point = getInnerPosition(p1, p2, p3);
                return point;
            }
        }
        return null;
    }

    public static void main(String[] args) {
        Circle c1 = new Circle(0, 0, 2);
        Circle c2 = new Circle(4, 0, 2);
        System.out.println("--------->" + getIntersect(c1, c2, null));
    }

}
